#include "car_config.h"     // 头文件
#include "car_mpu6050.h"    // 陀螺仪

// 陀螺仪数据
hi_u8 mpu_ds[14] = {0};
hi_u8 mpu_ln = 14;

hi_s32 mpu_acc[2] = {0};

hi_u8 mpu_b = 0;

// hi_double shuju[7] = {0};
hi_s16 shuju[7] = {0};

extern float Angle[3]; //角度数组

float Pitch_now;
float Roll_now;

#define GPIO0 3 //上1
#define GPIO1 8 //上2
#define GPIO2 2 //上3

//定时器数据
uint32_t exec1;
// 3861接入端口设置 i2c设置
hi_void Hi3861_Gpio_Init(hi_void)
{
    hi_u32 ret;

    hi_io_set_func(HI_IO_NAME_GPIO_13, HI_IO_FUNC_GPIO_13_I2C0_SDA);
    hi_io_set_func(HI_IO_NAME_GPIO_14, HI_IO_FUNC_GPIO_14_I2C0_SCL);

    ret = hi_i2c_init(HI_I2C_IDX_0, 400000);

    if (ret != HI_ERR_SUCCESS)
    {
        printf(" [hi_i2c_init] Failed \n");
    }

    printf("\n [Hi3861_Gpio_Init] \n");
}


// 任务前初始化
hi_void Car_Task_Init(hi_void)
{
    // 关闭 看门狗
    hi_watchdog_disable();

    // 3861接入端口设置 i2c设置
    Hi3861_Gpio_Init();

    // MPU6050 初始设置
    Mpu6050_Init();
    mpu_b = 0;

    //GPIO0引脚初始化 上1舵机
    IoTGpioInit(GPIO0);
    IoSetFunc(GPIO0,0);
    IoTGpioSetDir(GPIO0, IOT_GPIO_DIR_OUT);
    //GPIO1引脚初始化 上2舵机
    IoTGpioInit(GPIO1);
    IoSetFunc(GPIO1,0);
    IoTGpioSetDir(GPIO1, IOT_GPIO_DIR_OUT);
    printf("\n [Car_Task_Init] \n");
}

//舵机控制任务  任务优先级25
static void *CntrolDemo(const char *arg)
{
    (void)arg;
    while (1)
    {
        //printf("angle[1] = %f\n", Angle[1]);
        engine_turn1(-(int)Angle[1]);
        engine_turn2((int)Angle[0]);
    }
    return NULL;
}


//计算姿态角数据 任务优先级25
static void *Car_Task(const char *arg)
{
    (void)arg;

    // 初始设置
    Car_Task_Init();

    while (1)
    {
        Mpu6050_Measure_Sh(shuju);
        IMU_Update(shuju[4],shuju[5],shuju[6],shuju[0],shuju[1],shuju[2]);
        // printf("angle[1] = %f\n", Angle[1]);
        // engine_turn1(-Angle[1]);
    }

    return NULL;
}


// 应用入口
static void Car_Entry(void)
{
    //舵机控制任务  任务优先级25
    osThreadAttr_t sg90;

    sg90.name = "CntrolDemo";
    sg90.attr_bits = 0U;
    sg90.cb_mem = NULL;
    sg90.cb_size = 0U;
    sg90.stack_mem = NULL;
    sg90.stack_size = 1024; /* 堆栈大小为1024 */
    sg90.priority = 25;

    if (osThreadNew((osThreadFunc_t)CntrolDemo, NULL, &sg90) == NULL) {
        printf("[LedExample] Falied to create LedTask!\n");
    }


    //计算姿态角数据 任务优先级25
    osThreadAttr_t attr;

    attr.name = "Car_Task";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 1024 * 10;
    attr.priority = 25;

    if(osThreadNew((osThreadFunc_t)Car_Task, NULL, &attr) == NULL)
    {
        printf(" [Car_Entry] Falied to create Car_Task! \n");
    }
}

SYS_RUN(Car_Entry);




